﻿/*
 * lane_kalman.h
 *
 *  Created on: May 9, 2018
 *      Author: geoff
 */
#ifndef RMD_LANE_KALMAN_H_
#define RMD_LANE_KALMAN_H_

#include "data_frame.h"
#include "lane_const_parameter.h"
#include "lane_line_struct.h"
#include "lane_matrix.h"

namespace rmd {

struct Track_Para {
    bool flag_obsev;
    bool flag_first;
    bool flag_work;
    bool flag_is_good;
    bool flag_unconfirm;
    bool flag_stable;
    int time_no_obsev;
    int time_continu_obsev;
    int time_continu_good;
    long index;
    LaneLine Line;

    cv::Point Line_current[4];
    cv::Point Line_now[4];
    cv::Point Line_pre[4];

    short nQuality;
    bool  bIsTrue;
    bool angle_flagwork;
    int angle_cannotseetime;
};

class CKalman {
 private:
  Track_Para line_ekf_[kNumEkf];

  float lane_width;

 public:
  CKalman();
  ~CKalman();

  static CKalman* instance_;
  static CKalman* Instance();

  std::vector<LaneResult> Update(DataFrame* data_frame,
                               std::vector<LaneLine>& now_lines);
  void ExcludeOutKalmanLinesByCurrent(std::vector<LaneResult>& tempResPriSaveLine);
  void EkfInitialization();

 private:
 DataFrame* data_frame_;

 void EkfProcessSingleLine(LaneLine& lane_line);
 void TrackMainProcess(Track_Para& track_para);
 void SetFristObsevPara(Track_Para& track_para);
 void SetUpdatePara(Track_Para& track_para);

 cv::Point GetCenterPoint(cv::Point pt1, cv::Point pt2) ;

 public:
 static const int kStableTime;
 static const int kQualityTime;
 static const int kLinLooseMaxTime;
 static const int kContinuGood;
};
}
#endif
